#!/usr/bin/env python
# -*- coding: utf-8 -*-
# filename kalman.py
# Modify time: 2025/08/06 14:11

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter():
    # 参数说明：初始估计值、初始误差协方差、过程噪声、测量噪声、状态转移系数、观测系数
    def __init__(self, x0: float, P0: float, Q: float, R: float, F: float, H: float):
        self.x = x0
        self.P = P0
        self.Q = Q
        self.R = R
        self.F = F
        self.H = H

    def Update(self, z):
        # 预测步骤 状态预测 协方差预测
        # 状态预测：x = x_prev (假设系统模型为x_k = x_{k-1})
        x_prev = self.F * self.x
        # 估计误差协方差预测：P = P_prev + Q
        P_prev = self.F * self.P * self.F + self.Q

        # 更新步骤 测量残差 残差协方差 卡尔曼增益
        y = z - self.H * x_prev
        S = self.H * P_prev * self.H + self.R
        # 计算卡尔曼增益：K = P / (P + R)
        K = P_prev * self.H / S

        # 状态更新：x = x + K*(measurement - x)
        self.x = x_prev + K * y
        # 更新估计误差协方差：P = (1 - K)*P
        self.P = (1 - K * self.H) * P_prev

        return self.x

def main():
    N = 100
    # 生成100个均匀分布的点（0到2π之间）
    x = np.linspace(0, 2 * np.pi, N)

    # 生成正弦数据
    sine_data = 5*np.sin(x)

    # 生成噪声（均值为0，标准差为0.1的高斯噪声）
    noise = np.random.normal(0, 0.5, N)

    # 叠加噪声
    y = sine_data + noise

    kf = KalmanFilter(0.0, 1.0, 0.01, 0.05, 1.0, 1.0)
    filteredValue = []
    for i in range(N):
        filteredValue.append(kf.Update(y[i]))

    rc0 = y[0]
    k = 0.618
    rcFiltered = []
    for i in range(N):
        rc = k * rc0 + (1-k) * y[i]
        rcFiltered.append(rc)
        rc0 = rc

    # 方法1：全局设置字体（推荐）
    plt.rcParams["font.family"] = ["SimHei"]
    # 解决负号显示问题
    plt.rcParams['axes.unicode_minus'] = False  # 正确显示负号

    # 可视化结果
    plt.figure(figsize=(10, 6))
    plt.plot(x, sine_data, label='原始正弦曲线', color='blue', linestyle='--')
    plt.scatter(x, y, label='噪声数据点', color='red', alpha=0.6)
    plt.plot(x, filteredValue, label='卡尔曼滤波', color='orange')
    plt.plot(x, rcFiltered, label='RC滤波', color='cyan')
    plt.xlabel('x')
    plt.ylabel('y')
    plt.title('卡尔曼滤波')
    plt.legend()
    plt.grid(True)
    plt.show()


if __name__ == '__main__':
    main()
